/* Private includes ----------------------------------------------------------*/
#include "flux_observer.h"
#include "data.h"
#include "foc.h"
#include "user_lib.h"
/* Private define ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
FLUX_t flux;
/* Private function prototypes -----------------------------------------------*/

void Flux_Init(FLUX_t *flux)
{
  flux->Ts       = 0.00005f;  // 20k
  flux->Kp       = 2000.0f;
  flux->Ki       = 200000.0f;
  flux->gamma    = 18250000.0f;
  flux->phi_m_sq = MOTOR_Phi_m * MOTOR_Phi_m;
}

void Flux_Processing(FLUX_t *flux, FOC *foc)
{
  flux->y1 = -MOTOR_Rs * foc->i_alpha + foc->u_alpha;
  flux->y2 = -MOTOR_Rs * foc->i_beta + foc->u_beta;

  flux->x1 = flux->phi_alpha;
  flux->x2 = flux->phi_beta;

  float ls_i_alpha = MOTOR_Ls * foc->i_alpha;
  float ls_i_beta  = MOTOR_Ls * foc->i_beta;

  flux->eta_x1 = flux->x1 - ls_i_alpha;
  flux->eta_x2 = flux->x2 - ls_i_beta;

  float eta_x1_square;
  float eta_x2_square;
  eta_x1_square = flux->eta_x1 * flux->eta_x1;
  eta_x2_square = flux->eta_x2 * flux->eta_x2;

  flux->phi_err = flux->phi_m_sq - (eta_x1_square + eta_x2_square);

  flux->phi_err = Limit(flux->phi_err, flux->phi_m_sq);

  flux->d_x1 = flux->y1 + flux->gamma * flux->eta_x1 * flux->phi_err;
  flux->d_x2 = flux->y2 + flux->gamma * flux->eta_x2 * flux->phi_err;

  flux->phi_alpha += flux->d_x1 * flux->Ts;
  flux->phi_beta  += flux->d_x2 * flux->Ts;

  flux->pll_phi_alpha = flux->phi_alpha - ls_i_alpha;
  flux->pll_phi_beta  = flux->phi_beta - ls_i_beta;

  flux->eangle = atan2f(flux->pll_phi_beta, flux->pll_phi_alpha);
  if(flux->eangle < 0)
  {
    flux->eangle += 2 * PI;
  }
}

void Flux_PLL_Processing(FLUX_t *flux)
{
  flux->sin_pll = arm_sin_f32(flux->theta);
  flux->cos_pll = arm_cos_f32(flux->theta);

  flux->theta_err = -flux->pll_phi_alpha * flux->sin_pll + flux->pll_phi_beta * flux->cos_pll;

  flux->P  = flux->theta_err * flux->Kp;
  flux->I += flux->theta_err * flux->Ki;
  flux->I  = Limit(flux->I, 10000.0f);

  flux->omega = flux->P + flux->I;

  flux->theta += flux->omega * flux->Ts;
  flux->theta  = Limit_Angle(flux->theta);
}
